%Code to randomise placement of some object and then to detect the
%placement of it through MLE
%Authors: Nathan Rich Chris Chester
clear
clc
close all

N = 100; %Number of iterations
noisemult = 10; %Noise multiplier

% Pre-defining Arrays
R1 = zeros(4000,1);
R2 = zeros(4000,1);
S = zeros(4000,1);
Time = zeros(4000,1);
Out1 = zeros(4000,1);
Out2 = zeros(4000,1);
xguess = zeros(N,1);
yguess = zeros(N,1);
xguessmed = zeros(N,1);
yguessmed = zeros(N,1);

a1 = 0; %Position of Antenna relative to x axis, 0 on y axis
a2 = 1e-6; %antennas 1mm apart

freqwave = 8e9;    %8GHz Cosine wave used
velowave = 3e5; %speed of light km/s
lengthwave = velowave/freqwave;

%Both antenna first transmit a cosine then wait to receive it back from
%object.
srate = 80; %80 samples per wave

%Creating Signal to be sent
for I = 1:4000
    S(I) = cos(I*2*pi/srate); %Sent Signal
    Time(I) = I/(freqwave*srate)*1e9;  %Scales to ns
end

% %Showing Sent signal
% figure(1)
% plot(Time(1:200) ,S(1:200),'g')
% title('Sent Signal')
% xlabel('Time (ns)')
% ylabel('Amplitude')
% legend('Sent Signal')

%With ideal LPF
load Hd.mat;

% pause

for J = 1:N
    
    %Obtaining the recieved signal at antennas 1 and 2
    [R1, R2, delay1, delay2,objx,objy] = radar_signal(S,a1,a2,freqwave,velowave,srate,noisemult);
    d(J) = delay1;
    %Using cos(a)cos(a+b) = 1/2 cos(b) + 1/2 cos(2a), to find cos(b)
    Out1 = filter(Hd.Numerator,1,S.*R1);
    Out2 = filter(Hd.Numerator,1,S.*R2);
    
    %Averaging the DC of the filter (%%%%%*!MLE estimate!*%%%%%%)
    Out1ave = mean(Out1(1000:4000));   %First 1000 terms ignored to allow
    Out2ave = mean(Out2(1000:4000));   %for the signal to converge
    Out1med = median(Out1(1000:4000));
    Out2med = median(Out2(1000:4000));
    if Out1ave > 0.5
        Out1ave = 0.5;
    end
    if Out2ave > 0.5
        Out2ave =0.5;
    end
    if Out1med > 0.5
        Out1med = 0.5;
    end
    if Out2med > 0.5
        Out2med = 0.5;
    end
    
    %Calculating Phase difference
    phase1 = acos (Out1ave*2);
    phase2 = acos (Out2ave*2);
    phase1med = acos (Out1med*2);
    phase2med = acos (Out2med*2);
    p1(J) = phase1;
    p2(J) = phase2;
    %Checks to see if acos has chosen the right angle out of the two possible
    %choices
%     if (R1(srate*1/4) < 0) %sees if the wave is -ve at the pi/2 point
%         phase1 = 2* pi - phase1; % If the wave is -ve then the phase delay > pi
%         phase1med = 2* pi - phase1med;
%     end
%     
%     Repeats for send Receiver
%     if (R2(srate*1/4) < 0)
%         phase2 = 2* pi - phase2;
%         phase2med = 2* pi - phase2med;
%     end
    
    %Finds difference in distance for MLE
    phasedif = (phase1-phase2);
    pd(J) = phasedif;
    length_from_a1 = velowave*delay1/2; %d=v*t, wave travels path twice so divide by two
    len(J) = length_from_a1;
    length_from_a2 = velowave*delay2 - length_from_a1;  % wave travel a1 path aswell, so minus it
    len2(J) = length_from_a2;
    distdif = phasedif/(2*pi) * lengthwave; %d=(phase*wavelength)/(2pi) using d=vt, t*omega=phase, wavelength*f=v
    dd(J) = distdif;
    if distdif/(a2-a1) < -1 
        break
    end
    theta = acos(distdif/(a2-a1));
    t(J) = theta;
    xguess(J) = abs(length_from_a1*cos(theta));
    yguess(J) = abs(length_from_a1*sin(theta));
    
    %Finds difference in distance for median
    phasedifmed = (phase1med-phase2med);
    distdifmed = phasedifmed/(2*pi) * lengthwave; %d=(phase*wavelength)/(2pi) using d=vt, t*omega=phase, wavelength*f=v
    
    thetamed = acos(distdifmed/(a2-a1));
    
    xguessmed(J) = abs(length_from_a1*cos(thetamed));
    yguessmed(J) = abs(length_from_a1*sin(thetamed));
end

% %Showing the received signals
% figure(2)
% plot(Time(1:200),R1(1:200), 'b')
% hold on
% plot(Time(1:200),R2(1:200), 'r')
%
% title('Recieved signals')
% xlabel('Time (ns)')
% ylabel('Amplitude')
% legend('Received Signal 1', 'Received Signal 2')
%
% pause

x_new = mean(abs(xguess)); %%%%%%%MLE
y_new = mean(abs(yguess)); %%%%%%%MLE
xerror = abs(x_new - objx);
yerror = abs(y_new - objy);
totalerror = sqrt(xerror^2 + yerror^2);

xmedian = median(abs(xguess));
ymedian = median(abs(yguess));
xerrormed = abs(xmedian - objx);
yerrormed = abs(ymedian- objy);
totalerrormed = sqrt(xerrormed^2 + yerrormed^2);
%
% fprintf('Use trig to extract the phase\n')
% fprintf('cos(wt)*cos(wt + @) = 1/2 (cos(2wt) + cos(@)) \n\n')
% fprintf('Press Enter to continue\n\n')
% commandwindow
% pause

figure(3)
plot(Time,Out1)
title('Filtered Output')
xlabel('Time (ns)')
ylabel('Amplitude')


% pause
% clc
%
% fprintf('Gaussian Noise distribution\n')
% fprintf('f(x) = 1/ sqrt(2*pi*sigma^2) * exp( - (x - u)^2)/ (2*sigma^2) )\n\n')
% fprintf('Press Enter to continue\n\n')
% commandwindow
% pause
%
% fprintf('Likelihood function\n')
% fprintf('L = (2*pi)^(-n/2) / sigma^n * exp (- sum(xi - u)^2 / (2*sigma^2)) \n\n' )
% fprintf('Press Enter to continue\n\n')
% commandwindow
% pause
%
% fprintf('Log likelihood\n')
% fprintf('l = -1/2*n*ln(2*pi) - n*ln(sigma) - sum(xi - u)^2 / (2*sigma^2)\n\n')
% fprintf('Press Enter to continue\n\n')
% commandwindow
% pause
%
% fprintf('Maximum likelihood\n')
% fprintf( 'u^ = sum(xi)/n \n\n')
% fprintf('Press Enter to continue\n\n')
% commandwindow
% pause


[talnumsx,talvaluesx] = tally(xguess);
[talnumsy,talvaluesy] = tally(yguess);

xcoord1 = [a1, a2, objx];
ycoord1 = [0,0,objy];
xcoord2 = [x_new];
ycoord2 = [y_new];
xcoord3 = [xmedian];
ycoord3 = [ymedian];

%scatter graph to show estimation
figure(4)
scatter(xguess, yguess,50,'r','x')
hold on
scatter(xcoord1, ycoord1,'b','o')
title('Radar Estimation')
xlabel('distance (km)')
ylabel('distance (km)')
legend('Predicted','Actual','Location','NorthWest')

% pause

%scatter graph to show estimation
figure(5)
scatter(xcoord2, ycoord2,50,'r','x')
hold on
scatter(xcoord1, ycoord1,'b','o')
hold on
scatter(xcoord3, ycoord3,'g','+')
title('Radar Estimation')
xlabel('distance (km)')
ylabel('distance (km)')
legend('Predicted MLE','Actual','Predicted Median','Location','NorthWest')

figure(6)
bar(talvaluesx,talnumsx)
title('Distribution of X guesses')
xlabel('X distance')
ylabel('Number of Estimates')

figure(7)
bar(talvaluesy,talnumsy)
title('Distribution of Y guesses')
xlabel('Y distance')
ylabel('Number of Estimates')

fprintf('The total error for the MLE technique is %d km\n', totalerror);
fprintf('The total error for the median technique is %d km\n', totalerrormed);


